#include "LinearPrediction.h"

using namespace ypl;


/*!
 * @brief 向未来作预测
 * @param aSignal       -in-  已知信号
 * @param aPredicted    -out- 待预测信号，需指定待预测点数，即数组长度
 * @param iOrder        -in-  用来作线性预测的信号点数，不得超过已知信号点数。
 * 若超过，则取为原始信号点数
 */
void LinearPrediction::predictFuture(const Array1D<Real> &aSignal,
                                      Array1D<Real> &aPredicted,
                                      Integral iOrder)
{
    Integral n = aSignal.dim();
    Integral np = aPredicted.dim(); ///< 待预测的点数
    if (n ==0  || np == 0 || iOrder == 0)
        return;

    if (n < iOrder)
        iOrder = n; ///< 用于预测的点数最大不可超过原信号点数

    Array1D<Real> aCoef(iOrder);
    Real mse; ///< 最小总体平方差 （minimum total square error）
    calcPredictCoef(aSignal, aCoef, iOrder, mse);

    /*! 用指针代替数组，加速下标访问 */
    const Real* pSignal = aSignal;
    Real* pCoef = aCoef;
    Real* pPredicted = aPredicted;
    for (Integral i = 0; i < np; ++i) {  ///< 预测第 i 个点
        Real sum = 0;
        /*! Numerical Recipe 没有避免移动数据，建议使用循环数组以避免数据移动。
         *  此处直接利用下标变换规律，从原信号数组和预测出的信号数组读取数据，
         *  从而避免了移动数据，但有点晦涩。
         */
        for (Integral j = 0; j < i; ++j)  {
            sum += -pCoef[j] * pPredicted[i-1-j];
        }
        for (Integral j = i; j < iOrder; ++j) {
            sum += -pCoef[j] * pSignal[n-1-(j-i)];
        }
        pPredicted[i] = sum;
    }
}

/*!
 * @brief 向过去作预测
 * @param aSignal       -in-  已知信号
 * @param aPredicted    -out- 待预测信号，需指定待预测点数，即数组长度
 *        aPredicted[0..k] 按时间顺序排列，即
 * @param iOrder        -in-  用来作线性预测的信号点数，不得超过已知信号点数。
 * 若超过，则取为原始信号点数
 */
void LinearPrediction::predictPast(const Array1D<Real> &aSignal,
                                    Array1D<Real> &aPredicted,
                                    Integral iOrder)
{
    Integral n = aSignal.dim();
    Array1D<Real> aSignalReverse(n);
    const Real* pSignal = aSignal;
    Real* pSignalReverse = aSignalReverse;
    for (Integral i = 0; i < n; ++i) {
        pSignalReverse[i] = pSignal[n-1-i];
    }
    predictFuture(aSignalReverse, aPredicted, iOrder);

    /*! 预测结果倒序，使其为正常时间顺序的信号 */
    Real* pPredicted = aPredicted;
    Integral np = aPredicted.dim();
    Real tmp;
    Integral np1 = np-1;
    for (Integral i = 0; i < np/2; ++i) {
        tmp = pPredicted[i];
        pPredicted[i] =  pPredicted[np1-i];
        pPredicted[np1-i] = tmp;
    }
}

/*!
 * @brief 计算线性预测系数
 * @param aSignal       -in-  已知信号
 * @param aCoef         -out- 待计算的线性预测系数
 * @param iOrder        -in-  线性预测系数的个数，即用于预测的信号长度
 * @param mse           -out- 最小总体误差（minimum total square error）
 */
void LinearPrediction::calcPredictCoef(const Array1D<Real> &aSignal,
                                        Array1D<Real> &aCoef,
                                        Integral iOrder,
                                        Real &mse)
{
    Integral n = aSignal.dim();
    if (n < iOrder)
        iOrder = n;

    Array1D<Real> autoCorr; ///< 自相关向量
    calcAutoCorr(aSignal, autoCorr, iOrder);

    if (aCoef.dim() != iOrder)
        aCoef = Array1D<Real>(iOrder);

    Array1D<Real> aCoefOnTheWay(iOrder);

    Real* R = autoCorr;
    Real* a = aCoef;          ///< 当前得到的预测系数
    Real* aa = aCoefOnTheWay; ///< 前一步得到的预测系数

    mse = R[0];
    for (int i = 0; i < iOrder; ++i) {
        aa[i] = 0;
    }

    /*! 迭代 */
    for (int i = 1; i <= iOrder; ++i) {
        Real sum = 0;
        for (int j = 0; j < i-1; ++j) {
            sum += aa[j]*R[i-j-1];
        }
        Real ki = -(R[i] + sum) / mse;
        a[i-1] = ki;
        for (int j = 0; j < i-1; ++j) {
            a[j] = aa[j] + ki*aa[i-1-(j+1)]; ///< 小心：下标的转换很容易出错
        }
        mse *= (1- ki*ki);

        for (int k = 0; k <= i-1; ++k) {
            aa[k] = a[k];
        }
    }

}


/*!
 * @brief 计算信号的自相关
 * @param aSignal           -in-  信号
 * @param autoCorr          -out- 自相关系数，向量长度为 iOrder+1
 * @param iOrder            -in-  线性预测系数的个数，即用于预测的信号长度
 */
void LinearPrediction::calcAutoCorr(const Array1D<Real> &aSignal,
                                     Array1D<Real> &aAutoCorr,
                                     Integral iOrder)
{
    Integral n = aSignal.dim();

    if (aAutoCorr.dim() != iOrder)
        aAutoCorr = Array1D<Real>(iOrder+1);

    const Real* S = aSignal;
    Real* R = aAutoCorr;
    for (int i = 0; i <= iOrder; ++i) {
        Real sum = 0;
        for (int j = i; j < n; ++j) {
            sum += S[j]*S[j-i];
        }
        R[i] = sum;
        //std::cout << R[i] << std::endl;  // 调试用，自相关向量
    }
}
